//
// Created by dwx on 2024/1/30.
//
#include "MyProject.h"

ENCODER_S E0;

uint16_t ReadMT6701(void){
    uint16_t u16Data;

    M0_CS(0);
    HAL_SPI_TransmitReceive(&hspi3,(uint8_t*)&u16Data,(uint8_t*)&u16Data,1,HAL_MAX_DELAY);
    M0_CS(1);
    return (u16Data&0xfffc);
}

uint16_t getRawCount(void){
    uint16_t val;
    val = ReadMT6701();
    return val;
}

float getAngle(ENCODER_S *E){
    long angle_data,d_angle;
    angle_data = getRawCount();
    // tracking the number of rotations
    // in order to expand angle range form [0,2PI] to basically infinity
    d_angle = angle_data - E->angle_data_prev;
    // if overflow happened track it as full rotation
    if(fabs(d_angle)>(0.8f*E->cpr)){
        E->full_rotation_offset += (d_angle>0)?-_2PI:_2PI;
    }
    // save the current angle value for the next steps
    // in order to know if overflow happened
    E->angle_data_prev = angle_data;


//	if(E->full_rotation_offset >= ( _2PI*1000)) //转动圈数过多后浮点数精度下降，并导致堵转，每隔一千圈归零一次
//	{                                        //这个问题针对电机长时间连续转动；如果不是长时间一个方向转动也可以屏蔽掉这几句
//		E->full_rotation_offset=0;
//		E->angle_prev = E->angle_prev - _2PI*1000;
//	}
//	if(E->full_rotation_offset <= (-_2PI*1000))
//	{
//		E->full_rotation_offset=0;
//		E->angle_prev = E->angle_prev + _2PI*1000;
//	}


    // return the full angle
    // (number of full rotations)*2PI + current sensor angle

    return (E->full_rotation_offset + ((float )angle_data/E->cpr)*_2PI);
}

// Shaft velocity calculation
float getVelocity(ENCODER_S *E){
    long  now_us;
    float  Ts,angle_now,val;
    // calculate sample time
    now_us = HAL_GetTick();
    if(now_us < E->velocity_calc_timestamp)
    {
        Ts = (float )(E->velocity_calc_timestamp - now_us)/21*1e-6f;
    }
    else {
        Ts = (float)(0xffffff - now_us + E->velocity_calc_timestamp)/21*1e-6f;
    }
    // quick fix for strange cases (micros overflow)
    if(Ts == 0 || Ts > 0.5f){
        Ts = 1e-3f;
    }
    // current angle
    angle_now = getAngle(E);
    // velocity calculation
    val = (angle_now - E->angle_prev)/Ts;

    // save variables for future pass
    E->angle_prev = angle_now;
    E->velocity_calc_timestamp = now_us;
    return val;

}

void MagneticSensor_Init(void){
    E0.cpr = MT6701_CPR;
    usb_printf("MO_MT6701\n");

    delay_ms(10);
    E0.angle_data_prev = getRawCount();
    E0.full_rotation_offset = 0;
    delay_ms(5);
    E0.angle_prev = getAngle(&E0);
}